%% 初始化
clear all;
close all;
clc;
glvs
ts = 0.01;
avp0 = [[0;0;0]; [0;0;0]; glv.pos0];
xxx = [];
%% 姿态反解算轨迹生成
% seg = trjsegment(xxx, 'init',         0);
% seg = trjsegment(seg, 'uniform',     2*3600);
% seg = trjsegment(seg, 'turnleft',   45, 2);
% seg = trjsegment(seg, 'uniform',     1*3600);
seg = trjsegment(xxx, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   20, xxx, 5);
seg = trjsegment(seg, 'uniform',      10);
seg = trjsegment(seg, 'climb',        15, 2, xxx, 20);
seg = trjsegment(seg, 'uniform',      50);
seg = trjsegment(seg, '8turn',        360, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      50);
seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4);
for k=1:2
seg = trjsegment(seg, 'uniform',      380);
seg = trjsegment(seg, 'coturnright',  90, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      380);
seg = trjsegment(seg, 'coturnleft',   90, 2, xxx, 4);
end
seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      550);
seg = trjsegment(seg, 'coturnleft',   105, 180/105, xxx, 4);
seg = trjsegment(seg, 'uniform',      50);
seg = trjsegment(seg, 'descent',      15, 2, xxx, 20);
seg = trjsegment(seg, 'uniform',      30);
seg = trjsegment(seg, 'deaccelerate', 20, xxx, 5);
seg = trjsegment(seg, 'uniform',      3600-sum(seg.wat(:,1)));

%姿态反解算
damping = 1-exp(-ts/5.0);%增稳函数
% damping = 1;
att_b  = avp0(1:3); vn_b = avp0(4:6);pos_b = avp0(7:9);%初始载体姿态速度和位置
att_imu  = avp0(1:3); vn_imu = avp0(4:6);pos_imu = avp0(7:9);%初始陀螺系姿态速度和位置
eth = earth(pos_b,vn_b);Cbn_1 = a2mat(att_b)'; Cimun_1 = a2mat(att_imu)';  wm_1 = [0;0;0]; ts2 = ts/2;%初始位置初始化
%惯性导航算法参数设置
[nn, ts, nts] = nnts(2, ts);
cnt = 1;%对nn进行计数
count = 1;
%% 惯性导航误差设置
% att_sig = [0.2; 0.2; 1]*Unit.angleMin;%初始姿态误差
% vel_sig = [0.1;-3.9;0.1];%初始速度误差
% pos_sig = [20/WGS84Parameter.Re; 20/WGS84Parameter.Re; 10];%初始位置误差
% g_bias =[0.0014;0.0011;0.0007]./3600.*glv.deg;%陀螺仪零偏
g_bias =[0.01;0.01;0.01]./3600.*glv.deg;%陀螺仪零偏
% a_bias = [9.1625e-5;1.2622e-4;2.8438e-5];%加速度计零偏
% g_bias =[0;0;0].*glv.deg;%陀螺仪零偏
a_bias = [0;0;0];%加速度计零偏
%%添加高度误差
height_bias = 3;
%%
% g_noise = [0.002;0.002;0.002].*glv.deg;%陀螺仪噪声
% a_noise = [0.002;0.002;0.002];%加速度计噪声
g_noise = [0;0;0].*glv.deg;%陀螺仪噪声
a_noise = [0;0;0];%加速度计噪声
Kg = [0,0,0;0,0,0;0,0,0];%陀螺仪比例因子误差矩阵
Ka = [0,0,0;0,0,0;0,0,0];%加速度计比例因子误差矩阵

Cg = [0,0,0;0,0,0;0,0,0];%陀螺仪安装关系误差矩阵
Ca = [0,0,0;0,0,0;0,0,0];%加速度计安装关系误差矩阵
I33 = eye(3);

%% 惯性导航初始化
ins = insinit(avp0, ts);
%% 旋转调制相关参数声明及初始化
rotation_time = 15;% 6 12
hold_on_period = 15;
rotation_w_deg = 180/rotation_time;
rotation_w_rad = rotation_w_deg * glv.deg;
rotation_process = 16;
whole_period = rotation_process* (rotation_time+hold_on_period);
lasting = 0;%系统持续时间
state = 0;%旋转调制所在状态
whether_static = 0;
%% 进度条相关函数
len = fix(sum(seg.wat(:,1))/ts);%求出总共的时长
ki = timebar(1, len, 'Trajectory Simulation.');%进度条
wvm = zeros(nn,6);
%% 惯性导航容器
w_imu = zeros(3,1);% imu旋转相关参数
ins_real = zeros(len,10);
ins_cal = zeros(fix(len/nn),10);
rotation_info = zeros(fix(len/nn),3);
rotation_info(1,:) = [0,0,rotation_w_rad];
%%
%反向陀螺加速度计解算
for k = 1:1:size(seg.wat,1)
    wt = seg.wat(k,3:5)';at = seg.wat(k,6:8)';%获得行数和列数
    wt_tmp = wt;%临时变量存储
    ufflag = 0;
    if norm(wt) ==0 && norm(at)==0%只有在函数到0的时候才会启用这个ufflag,在描述为0时将惯性导航拉回原函数
        ufflag = 1;
        vnr_b = a2mat(att_b)*[0;seg.wat(k,2);0];%速度运动阻尼
        vnr_imu = a2mat(att_imu)*[0;seg.wat(k,2);0];%速度运动阻尼
    end
    for i = 1:1:(round((seg.wat(k,1)/ts)))
%         [w_imu,rotation_info(ki,:)] = sixteenRotationStatic(seg,i,k,wt_tmp,ts,whole_period,rotation_time,hold_on_period,rotation_w_rad);
        if ki ~= 1    
%             [w_imu,rotation_info(ki,:)] = sixteenRotationMove(seg,i,k,wt_tmp,ins.wnb,rotation_info(ki-1,:)',ts,whole_period,rotation_time,hold_on_period,rotation_w_rad);
            [w_imu,rotation_info(ki,:)] = sixteenRotationMove(seg,i,k,ki,wt_tmp,wt,[0;0;0],ts,whole_period,rotation_time,hold_on_period,rotation_w_rad);
        else
            w_imu = [0;0;rotation_w_rad];
            rotation_info(ki,:) = [0,0,rotation_w_rad];
        end
        %% 载体系的运动
        sa_b = sin(att_b);ca_b = cos(att_b);
        si_b = sa_b(1);sk_b = sa_b(3);ci_b = ca_b(1);ck_b = ca_b(3);
        Cnt_b = [ ck_b, -ci_b*sk_b, si_b*sk_b;
            sk_b, ci_b*ck_b, -si_b*ck_b;
            0,  si_b,     ci_b];%只有y轴的旋转不会影响加速度的分量，其余的姿态，均会影响加速度的分量
        att_b = att_b + wt * ts;
        %             att = att + (wt)*ts;
        Cnb = a2mat(att_b);
        if ufflag == 1%这个应该就是接近0时候的增稳函数，为了使数据回到0处
            na = norm(vn_b-vnr_b)/ts; maxa = 0.1;
            if na<maxa, na= maxa; end
            vn1_b = vn_b - damping/na*(vn_b-vnr_b); an_b = (vn1_b - vn_b)/ts;
            vn01_b = (vn_b+vn1_b)/2;  vn_b = vn1_b;
        else
            %%%%%向心加速度平滑滤波
            %                 if at(1) ~= 0 && wt(3) ~=0%沿着水平转弯
            %                     logistic = 1/(1+exp(-10*(i*ts)+5));
            %                     an = Cnt*(diag([logistic,1,1])*at);%这个的目的应该时将加速度进行分解
            %                     vn1 = vn + an*ts;  vn01 = (vn+vn1)/2;  vn = vn1;%在导航系下面获得的飞机在导航坐标系运动的参数
            % %                 elseif at(3) ~= 0 && wt(1) ~=0%沿着俯仰转弯
            % %                     logistic = 1/(1+exp(-10*(i*ts)+5));
            % %                     an = Cnt*(diag([1,1,logistic])*at);%这个的目的应该时将加速度进行分解
            % %                     vn1 = vn + an*ts;  vn01 = (vn+vn1)/2;  vn = vn1;%在导航系下面获得的飞机在导航坐标系运动的参数
            %                 else
            an_b = Cnt_b*at;%这个的目的应该时将加速度进行分解
            vn1_b = vn_b + an_b*ts;  vn01_b = (vn_b+vn1_b)/2;  vn_b = vn1_b;%在导航系下面获得的惯性导航在导航坐标系运动的参数
            %                 end
            
        end
        %% imu坐标系
        %%%%%%%%%%%%%%%%%%%%imu系的运动以及参数输出%%%%%
        
        sa_imu = sin(att_imu);ca_imu = cos(att_imu);
        si_imu = sa_imu(1);sk_imu = sa_imu(3);ci_imu = ca_imu(1);ck_imu = ca_imu(3);
        Cnt_imu = [ ck_imu, -ci_imu*sk_imu, si_imu*sk_imu;
            sk_imu, ci_imu*ck_imu, -si_imu*ck_imu;
            0,  si_imu,     ci_imu];%只有y轴的旋转不会影响加速度的分量，其余的姿态，均会影响加速度的分量
        att_imu = att_imu + (wt+w_imu)*ts;
        %             att = att + (wt)*ts;
        Cnimu = a2mat(att_imu);
        if ufflag == 1%这个应该就是接近0时候的增稳函数，为了使数据回到0处
            na = norm(vn_imu-vnr_imu)/ts; maxa = 0.1;
            if na<maxa, na= maxa; end
            vn1_imu = vn_imu - damping/na*(vn_imu-vnr_imu); an_imu = (vn1_imu - vn_imu)/ts;
            vn01_imu = (vn_imu+vn1_imu)/2;  vn_imu = vn1_imu;
        else
            %%%%%向心加速度平滑滤波
            %                 if at(1) ~= 0 && wt(3) ~=0%沿着水平转弯
            %                     logistic = 1/(1+exp(-10*(i*ts)+5));
            %                     an = Cnt*(diag([logistic,1,1])*at);%这个的目的应该时将加速度进行分解
            %                     vn1 = vn + an*ts;  vn01 = (vn+vn1)/2;  vn = vn1;%在导航系下面获得的飞机在导航坐标系运动的参数
            % %                 elseif at(3) ~= 0 && wt(1) ~=0%沿着俯仰转弯
            % %                     logistic = 1/(1+exp(-10*(i*ts)+5));
            % %                     an = Cnt*(diag([1,1,logistic])*at);%这个的目的应该时将加速度进行分解
            % %                     vn1 = vn + an*ts;  vn01 = (vn+vn1)/2;  vn = vn1;%在导航系下面获得的飞机在导航坐标系运动的参数
            %                 else
            an_imu = Cnt_imu*at;%这个的目的应该时将加速度进行分解
            vn1_imu = vn_imu + an_imu*ts;  vn01_imu = (vn_imu+vn1_imu)/2;  vn_imu = vn1_imu;%在导航系下面获得的惯性导航在导航坐标系运动的参数
            %                 end
            
        end
        %%%%%%%%%%%%%%%%%%%%%
        %%
        %% 位置更新
        dpos01 = [vn01_b(2)/eth.RMh;vn01_b(1)/eth.clRNh;vn01_b(3)]*ts2;%经纬高(准确来说是经纬高增量，参数化成椭圆模型。
        eth = earth(pos_b+dpos01, vn01_b);%地球一些参数的更新，使用运动过程中运动一半的经纬高进行补偿包括wien wenn 所在地的半径转换相关参数
        pos_b = pos_b + dpos01*2;%前面是更新eth时使用的时一般的增量，位置更新时使用完整位置进行更新
        %% imu输出更新
        phim = m2rv(Cimun_1*Cnimu) + (Cimun_1+Cnimu')*(eth.wnin*ts2);%m2rv获得末向量相对初向量的旋转矢量+地球自转补偿？
        %             wm =(I33+Kg)*(I33+Cg)*(glv.I33+askew(wm_1/12))^-1*phim+g_bias.*ts+g_noise.*randn(3,1).*ts; %陀螺仪的增量 圆锥误差补偿应该是%添加了陀螺仪误差模型
        wm =(I33+Kg)*(I33+Cg)*(glv.I33+askew(wm_1/12))^-1*phim+g_bias.*ts;
        dvbm = Cimun_1*(rv2m(eth.wnin*ts2)*(an_b-eth.gcc)*ts);%根据比例方程反推 这个里面的wnin时wnin+wnie = 2wnie +wnen 估计是为了计算格式加速度
        %             vm = (I33+Ka)*(I33+Ca)*(glv.I33+askew(wm/2))^-1*dvbm+a_bias.*ts+a_noise.*randn(3,1).*ts;   % 加速度计增量 旋转误差补偿%添加了加速度计误差模型
        vm = (I33+Ka)*(I33+Ca)*(glv.I33+askew(wm/2))^-1*dvbm+a_bias.*ts;   % 加速度计增量 旋转误差补偿%添加了加速度计误差模型
        %一些时间函数相关内容（时间进度条）
        %             vm_2=vm_1;vm_1 = vm;
        kts = ki*ts;
        avp = [att_b;vn_b;pos_b;kts]';
        ins_real(ki,:) = avp;
        imu = [wm;vm;kts]';
        wm_2=wm_1;
        wm_1 = wm;Cimun_1 = Cnimu';
        wvm(cnt,:) = imu(1,1:6);
        %%
        %进行正向的解算
        
        if cnt == nn
            ins = insupdate(ins, wvm);
            %静止抑制天向发散
            ins.pos(3) = pos_b(3)+height_bias;
            ins.avp(6) = 0;
%             ins.vn(3) = 0;
            %                 ins.avp(9) = avp0(9)+height_bias;
            cnt = 0;
            wvm = zeros(nn,6);
            %                 ins.avp(1:3,1) = a2mat(-rotation_info'.*ts)*ins.avp(1:3,1);
            %                 ins.avp(4:6,1) = a2mat(-rotation_info'.*ts)*ins.avp(4:6,1);
            ins_cal(ki/nn,:) = [ins.avp',kts];
            %                 dxyz1 = pos2dxyz(ins.avp(7:9,:)');
            %                 plot(dxyz1(:,1), dxyz1(:,2),'b'); xygo('est', 'nth');
            %                 hold on;
        end
        cnt = cnt + 1;
        ki = timebar;
        kj = ki-1;
        count = count +1;
    end
end

%直观感受和真实值以及严老师的程序相差的大小
trj = trjsimu(avp0, seg.wat, ts, 1);
ins0 = insinit(avp0, ts);
len = length(trj.imu);
ins_yan = zeros(fix(len/nn),10);
for k=1:nn:len-nn+1
    k1 = k+nn-1;
    wvm = trj.imu(k:k1,1:6)+[g_bias',a_bias'].*ts;
    %     wvm = trj.imu(k:k1,1:6);
    %     t = trj.imu(k1,end);
    ins0 = insupdate(ins0, wvm);
    ins0.avp(6) = 0;
    %     ins0.avp(9) = avp0(9);
    ins.pos0(3) = pos_b(3)+height_bias;
    ins_yan(fix(k/nn)+1,:)= [ins0.avp',k*ts];
end
